#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <stdbool.h>
#include <stdint.h>
#include <omp.h>



#define DUMMY  do{}while(0)

#define CONCAT(A,B) A##B

#ifdef __x86_64__
#define GET_ESP(X) void * __esp_val;\
		__asm__ __volatile__ (\
			"mov %%rsp,%0" : "=r"(__esp_val) ); \
		X = __esp_val
#elif __i386__
#define GET_ESP(X) void * __esp_val;\
		__asm__ __volatile__ (\
			"mov %%esp,%0" : "=r"(__esp_val) ); \
		X = __esp_val
#else 
 #error "Unsupported arch"
#endif

int f( int a) {
	return 2*a;

}


// TODO : modulariser l'accès à ces variables.
static __thread void * helper_reg = 0;
static __thread void * helper_out = NULL;
static __thread size_t helper_size = 0;
void  helper( long first )  {
	long start = (long)&first;
	void * res = malloc( (long)helper_reg - start );
	memcpy( res, &first, (long)helper_reg - start );
	helper_size = (long)helper_reg - start;
	helper_out = res;
}



#define delegate_withlock( lock,f,...) \
	do { delegate_withlock_1( __LINE__, lock, f, ##__VA_ARGS__ ); } while(0)

#define delegate_withlock_1( line, lock, f,... ) \
	GET_ESP(helper_reg);\
        void CONCAT( local_func,line) () {\
                __typeof(f) z = ((__typeof(f))&helper);\
                z( __VA_ARGS__ );\
        }\
        CONCAT(local_func,line)();\
	delegate_lock_do( lock, omp_get_thread_num(), f, helper_out, helper_size )

static void unpack_do( void * f, void * arg, size_t size_args ) {
	volatile char buf[size_args];
	memset( (char*)buf, 0, size_args );
	void * a;
	GET_ESP(a);
	if ( (unsigned long)&f - (unsigned long)a < size_args ) {
		fprintf(stderr, "Erreur !!");
		exit(EXIT_FAILURE );
	}	
	void (*z)() = (void (*)())f;
	memcpy( a,arg, size_args );
	z();
	
}


struct delegate_task {
    struct delegate_task * next;
    void (*op)(void*);
    void * arg;
    size_t argsize;
};

union dwordcas {
	struct {
	uint16_t guard;
	uint16_t count;
	} splitted;
	uint32_t stamp;
};


struct delegate_lock {
    union dwordcas dwordcas;
    int owner;
    int fifo_guard;
    struct delegate_task * fifo_head;
    struct delegate_task * fifo_end;
};

typedef volatile struct delegate_lock  * delegate_lock_p;


delegate_lock_p new_delegate_lock() {
    delegate_lock_p res =  calloc( 1, sizeof( struct delegate_lock));
    res->owner = -1;
    return res;

}

void delegate_lock_do(  delegate_lock_p  lock, int thread_id, void *op, void * arg, size_t argsize )  {
    int res;
    union dwordcas localdwcas_sav= lock->dwordcas;
    union dwordcas localdwcas= localdwcas_sav;
    
    do { 
	localdwcas_sav= lock->dwordcas;
        localdwcas= localdwcas_sav;
	localdwcas.splitted.guard += 1;
	localdwcas.splitted.count += 1;
	res = localdwcas_sav.splitted.guard;
    } while (!__sync_bool_compare_and_swap( &lock->dwordcas.stamp, localdwcas_sav.stamp, localdwcas.stamp  ));

    if ( res == 0 ) {
        lock->owner = thread_id;
    } else if ( lock->owner == thread_id ) {
        // Succeed -reentrancy
    } else {
        // Fail
        struct delegate_task * a = calloc( 1, sizeof( struct delegate_task) );
        a->op = op;
        a->arg = arg;
        a->argsize = argsize;

        while (  !__sync_bool_compare_and_swap(&lock->fifo_guard, 0, 1) ) {
            while ( lock->fifo_guard );
        }
        a->next = NULL;
        if ( lock->fifo_end != NULL ) {
            lock->fifo_end->next = a;
        } else {
            lock->fifo_head = a;
        }
        lock->fifo_end = a;
	__sync_synchronize();
        lock->fifo_guard = 0;

	//__sync_fetch_and_sub( &lock->guard, 1 );
    	union dwordcas localdwcas_sav = lock->dwordcas;
    	union dwordcas localdwcas = localdwcas_sav;
	do {
    		localdwcas_sav= lock->dwordcas;
   	 	localdwcas = localdwcas_sav;
		
		localdwcas.splitted.guard -= 1;
	
	} while ( !__sync_bool_compare_and_swap( &lock->dwordcas.stamp, localdwcas_sav.stamp, localdwcas.stamp ));

	return ;	    
   }


    unpack_do( op, arg, argsize );
    free( arg );
    // Succeed : process requests :
    
    while ( true ) {
	 while ( lock->fifo_head == NULL)  { 
	// Go out atomically, not leaving tasks behind.
		union dwordcas localdwcas = lock->dwordcas;
		union dwordcas localdwcas_new = localdwcas;
		localdwcas_new.splitted.guard = 0;
		if ( localdwcas.splitted.guard == 1 && lock->fifo_head == NULL ) {
    			lock->owner = -1;
    			if ( __sync_bool_compare_and_swap( &lock->dwordcas.stamp, localdwcas.stamp, localdwcas_new.stamp) ) {
				 goto end;
			}
    			lock->owner = thread_id;
		}
	}
        unpack_do( lock->fifo_head->op, lock->fifo_head->arg, lock->fifo_head->argsize );
	// Locks the fifo.
        while (  !__sync_bool_compare_and_swap(&lock->fifo_guard, 0, 1) ) {
            while ( lock->fifo_guard );
        }
        free( lock->fifo_head->arg );
	struct delegate_task * tmp = lock->fifo_head;
        lock->fifo_head = lock->fifo_head->next;
	
        free( tmp );
	if ( lock->fifo_head == NULL ) {
		lock->fifo_end = NULL;
	}

	__sync_synchronize();
	// Unlocks the fifo.
	lock->fifo_guard = 0; 
    }
end :
   DUMMY;

    // Ended :
}


// TESTING


static volatile int counter = 0;


static void adder( int a, int b, int c, long long int d ) {
	printf( "Adder : %d %d %d %lld\n", a,b,c,d );

	counter += a;
	counter += b;
	counter += c;
	counter +=d;
}


static delegate_lock_p lock;

int main() {
	lock = new_delegate_lock();

	#pragma omp parallel
{
	for ( int i = 0 ; i < 10000; ++ i ) { 
	delegate_withlock( lock, &adder, 1, 1, 1, 1 );
	}

}
	printf( "Res : %d \n", counter );
	
	
}
